  
/*
Arduino CNC电机扩展板驱动NEMA17步进电机示例程序-1
By 太极创客（http://www.taichi-maker.com）
2019-03-10
 
本示例程序旨在演示如何使用Arduino Uno开发板通过Arduino CNC电机扩展板来驱动NEMA17步进电机（42步进电机）。
 
如需获得更多关于本示例程序的电路连接以及CNC电机扩展板的资料信息，
请参考太极创客网站（http://www.taichi-maker.com），并在首页搜索栏中搜索关键字：CNC扩展板
*/
#include <AccelStepper.h>  //本示例程序使用AccelStepper库
#include "ik.h"
#include <FlexiTimer2.h>
#include "CmdMessenger.h"

const int buttonPin = 47;     // the number of the pushbutton pin
// 定义电机控制用常量

//
// int xdirPin = A1;//大臂步进电机方向引脚
//int xstepperPin = A0;//大臂步进电机脉冲引脚
//int xen=38;//大臂步进电机使能引脚
//int xstop=2;//大臂步进电机限位器向引脚
// A4988连接Arduino引脚号
const int xdirPin = A1;     // 方向控制引脚
const int xstepPin = A0;    // 步进控制引脚
const int xenablePin = 38;  // 使能控制引脚
const int xstop=2;//大臂步进电机限位器向引脚


int ydirPin = A7;//小臂步进电机方向引脚
int ystepPin = A6;//小臂步进电机脉冲引脚
int yenablePin=A2;//小臂步进电机使能引脚
int ystop=15;//小臂步进电机限位器向引脚

int zdirPin = 48;//旋转步进电机方向引脚
int zstepPin = 46;//旋转步进电机脉冲引脚
int zenablePin=A8;//旋转步进电机使能引脚
int zstop=19;//旋转步进电机限位器引脚


const float x_ratio = (360/1.8*13.7*16/360);
const float y_ratio = (360/1.8*13.7*16/360);
const float z_ratio = (360/1.8*5.17*16/360);

const int moveSteps = 2000;    //运行步数
const float maxSpeed = 1200;

double targetRot = 0;
double targetLeft = 0;
double targetRight = 0;

double current_x = 0;
double current_y = 0;
double current_z = 0;

/* Define available CmdMessenger commands */
enum {
    move_to,
    move_to_retult,
};

/* Initialize CmdMessenger -- this should match PyCmdMessenger instance */
const int BAUD_RATE = 9600;
CmdMessenger c = CmdMessenger(Serial2,',',';','/');


AccelStepper stepper1(1,xstepPin,xdirPin);//建立步进电机对象
AccelStepper stepper2(1,ystepPin,ydirPin);//建立步进电机对象
AccelStepper stepper3(1,zstepPin,zdirPin);//建立步进电机对象

/* callback */
void on_move_to(void){
//  Serial.print("on");
    /* Grab two integers */
    int value1 = c.readBinArg<int>();
    int value2 = c.readBinArg<int>();
    int value3 = c.readBinArg<int>();
    /* Send result back */ 
//    xyzToAngle(200, 0, 100, targetRot, targetLeft, targetRight, false);
    xyzToAngle(value1, value2, value3, targetRot, targetLeft, targetRight, false);
    moveTo(targetLeft, targetRight, targetRot);
    c.sendBinCmd(move_to_retult, 1);
}

/*
 * x stepper1.moveTo(90.0 - x_angle)
 * y stepper2.moveTo(16.5 + y_angle)
*/
void moveTo(float x_angle, float y_angle, float z_angle) {
//  Serial.println((90.0 - x_angle) * x_ratio);
//  Serial.println(x_ratio);

//  float max_angle = 0;
//  float delta_x = fabs(current_x - x_angle);
//  float delta_y = fabs(current_y - y_angle);
//  float delta_z = fabs(current_z - z_angle);
//
//  max_angle = delta_x > delta_y ? delta_x:delta_y;
//  max_angle = max_angle > delta_z ? max_angle:delta_z;
//
//  stepper1.setSpeed(maxSpeed * delta_x / max_angle);
//  stepper1.setSpeed(maxSpeed * delta_y / max_angle);
//  stepper1.setSpeed(maxSpeed * delta_z / max_angle);
//  Serial.println(maxSpeed * delta_x / max_angle);
//  Serial.println(maxSpeed * delta_y / max_angle);
//  Serial.println(maxSpeed * delta_z / max_angle);

  stepper1.moveTo(-((90.0 - x_angle) * x_ratio));
  stepper2.moveTo(((16.5 + y_angle) * y_ratio));
  stepper3.moveTo(((90.0 + z_angle) * z_ratio));

  current_x = x_angle;
  current_y = y_angle;
  current_z = z_angle;

//  while (stepper1.isRunning());
//  while (stepper2.isRunning());
//  while (stepper3.isRunning());
  
}

bool pick = true;

void flash() {
  if (pick) {
    xyzToAngle(200, 0, 100, targetRot, targetLeft, targetRight, false);
    moveTo(targetLeft, targetRight, targetRot);
    pick = false;
  } else {
    xyzToAngle(200, 0, 130, targetRot, targetLeft, targetRight, false);
    moveTo(targetLeft, targetRight, targetRot);
    pick = true;
  }
}

/* Attach callbacks for CmdMessenger commands */
void attach_callbacks(void) {
    c.attach(move_to,on_move_to);
}

void action() {
  xyzToAngle(250, 0, 200, targetRot, targetLeft, targetRight, false);
  moveTo(targetLeft, targetRight, targetRot);
  while (stepper1.isRunning() || stepper2.isRunning() || stepper3.isRunning()) {
    stepper1.run();
    stepper2.run();
    stepper3.run();
  }

  xyzToAngle(250, 0, 30, targetRot, targetLeft, targetRight, false);
  moveTo(targetLeft, targetRight, targetRot);
  while (stepper1.isRunning() || stepper2.isRunning() || stepper3.isRunning()) {
    stepper1.run();
    stepper2.run();
    stepper3.run();
  }

//  for (int i = 200; i > 30; i-=50) {
//     xyzToAngle(250, 0, i, targetRot, targetLeft, targetRight, false);
//     moveTo(targetLeft, targetRight, targetRot);
//     while (stepper1.isRunning() || stepper2.isRunning() || stepper3.isRunning()) {
//      stepper1.run();
//      stepper2.run();
//      stepper3.run();
//    }
//  }
}


void setup() {

  Serial.begin(9600);
//  Serial2.begin(9600);
  
  pinMode(xstepPin,OUTPUT);     // Arduino控制A4988步进引脚为输出模式
  pinMode(xdirPin,OUTPUT);      // Arduino控制A4988方向引脚为输出模式
  pinMode(xenablePin,OUTPUT);   // Arduino控制A4988使能引脚为输出模式
  digitalWrite(xenablePin,LOW); // 将使能控制引脚设置为低电平从而让
                                // 电机驱动板进入工作状态
//
  pinMode(ystepPin,OUTPUT);     // Arduino控制A4988步进引脚为输出模式
  pinMode(ydirPin,OUTPUT);      // Arduino控制A4988方向引脚为输出模式
  pinMode(yenablePin,OUTPUT);   // Arduino控制A4988使能引脚为输出模式
  digitalWrite(yenablePin,LOW); // 将使能控制引脚设置为低电平从而让
                                // 电机驱动板进入工作状态
  pinMode(zstepPin,OUTPUT);     // Arduino控制A4988步进引脚为输出模式
  pinMode(zdirPin,OUTPUT);      // Arduino控制A4988方向引脚为输出模式
  pinMode(zenablePin,OUTPUT);   // Arduino控制A4988使能引脚为输出模式
  digitalWrite(zenablePin,LOW); // 将使能控制引脚设置为低电平从而让
//                                 电机驱动板进入工作状态
 
  stepper1.setMaxSpeed(1400.0);     // 设置电机最大速度300 
  stepper1.setAcceleration(1600.0);  // 设置电机加速度20.0  
  

  stepper2.setMaxSpeed(1600.0);     // 设置电机最大速度300 
  stepper2.setAcceleration(1800.0);  // 设置电机加速度20.0  


  stepper3.setMaxSpeed(1600.0);     // 设置电机最大速度300 
  stepper3.setAcceleration(800.0);  // 设置电机加速度20.0  

  pinMode(xstop, INPUT_PULLUP); //X轴 STOP
  pinMode(ystop, INPUT_PULLUP); //X轴 STOP
  pinMode(zstop, INPUT_PULLUP); //X轴 STOP

  pinMode(buttonPin, INPUT);

  stepper1.setSpeed(1000);

  Serial2.begin(BAUD_RATE);
  attach_callbacks();    
//  Serial.println("start");
  while(1) {
    if(digitalRead(xstop)==0) {
      stepper1.stop();
      stepper1.setCurrentPosition(0);
      break;
    } else{
      stepper1.runSpeed();
    }
  }

  stepper2.setSpeed(-1000);
  while(1) {
    if(digitalRead(ystop)==0) {
      stepper2.stop();
      stepper2.setCurrentPosition(0);
      break;
    } else{
      stepper2.runSpeed();
    }
  }

  stepper3.setSpeed(-1000);
  while(1) {
    if(digitalRead(zstop)==0) {
      stepper3.stop();
      stepper3.setCurrentPosition(0);
      break;
    } else{
      stepper3.runSpeed();
    }
  }

  int curr_x = 200;
  int curr_y = 0;
  int curr_z = 100;
//  xyzToAngle(curr_x, curr_y, curr_z, targetRot, targetLeft, targetRight, false);
//  Serial.println(targetLeft);
//  Serial.println(targetRight);
//  Serial.println(targetRot);
//  moveTo(targetLeft, targetRight, targetRot);
//  delay(10000);
  action();
  FlexiTimer2::set(5000, flash); // MsTimer2 style is also supported

  // FlexiTimer2::start();
  
}



void loop() {
  // 控制步进电机往复运动
//  if ( stepper1.currentPosition() == 0 ){ 
//    stepper1.moveTo(-10960);              
//  } 
//  else if ( stepper1.currentPosition() == -10960  ){
//    stepper1.moveTo(0);            
//  }         
  stepper1.run();
  stepper2.run();
  stepper3.run();

//  Serial.println(stepper2.isRunning());

  c.feedinSerialData();

  int buttonState = digitalRead(buttonPin);
  if (buttonState == LOW) {
    stepper1.stop();
    stepper2.stop();
    stepper3.stop();
  }
  
//  Serial2.print("loop");
//  if ( stepper2.currentPosition() == 0 ){ 
//    stepper2.moveTo(moveSteps);              
//  } else if ( stepper2.currentPosition() == moveSteps  ){
//    stepper2.moveTo(0);            
//  }         
//  moveTo(45.0, 0, 0);
}
